#include "LdPcl.h"
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/registration/sample_consensus_prerejective.h>

typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

// Align a rigid object to a scene with clutter and occlusions
int main(int argc, char **argv)
{
    // Point clouds
    PointCloudT::Ptr object(new PointCloudT);
    PointCloudT::Ptr object_aligned(new PointCloudT);
    PointCloudT::Ptr scene(new PointCloudT);
    FeatureCloudT::Ptr object_features(new FeatureCloudT);
    FeatureCloudT::Ptr scene_features(new FeatureCloudT);

    std::vector<std::string> BroadCode;
    BroadCode.push_back("000000000000001");
    LdsLidar &mylidar = LdsLidar::GetInstance();
    int init = mylidar.InitLdsLidar(BroadCode, 40000);
    sleep(1);
    while (!init)
    {
        init = mylidar.InitLdsLidar(BroadCode, 40000);
        sleep(0.1);
    }
    char key;
    std::vector<Pcloud> Clouds;
    int count = 0;
    while (key != 's' && count != 3)
    {
        Pcloud toutput(new Cloud());
        Pcloud Temp(new Cloud());
        mylidar.getcloud(Temp);
        while (Temp == nullptr || Temp->size() < 30000)
            mylidar.getcloud(Temp);
        pcl::visualization::CloudViewer V_origin("Origin");
        V_origin.showCloud(Temp);
        while(!V_origin.wasStopped()){}

        Filter(Temp, toutput);
        Clouds.push_back(toutput);

        key = cv::waitKey(1);
        pcl::visualization::CloudViewer V_per("Viewer per Cloud");
        V_per.showCloud(toutput);
        while (!V_per.wasStopped())
        {
        }
        count++;
    }

    // Downsample
    Pcloud All_clouds(new Cloud());
    Pcloud All_clouds_(new Cloud());
    *All_clouds = *(Clouds[0]);
    *All_clouds_ = *(Clouds[0]);
    //将初始点云存到object
    object->resize(Clouds[0]->size());
    for (size_t i = 0; i < (*Clouds[0]).size(); i++)
    {
        PointNT tPoint;
        tPoint.x = Clouds[0]->points[i].x;
        tPoint.y = Clouds[0]->points[i].y;
        tPoint.z = Clouds[0]->points[i].z;
        object->points.push_back(tPoint);
    }
    pcl::NormalEstimationOMP<PointNT, PointNT> nestobj;
    nestobj.setRadiusSearch(0.01);
    nestobj.setInputCloud(object);
    nestobj.compute(*object);

    //将每一个点云存到scene
    for (size_t i = 1; i < Clouds.size(); i++)
    {
        for (size_t j = 0; j < Clouds[i]->size(); j++)
        {
            PointNT tPoint;
            tPoint.x = Clouds[i]->points[j].x;
            tPoint.y = Clouds[i]->points[j].y;
            tPoint.z = Clouds[i]->points[j].z;
            scene->points.push_back(tPoint);
        }
        Fusion(Clouds[i], All_clouds_); //对照组，没有经过RoT变换
        std::cout << "对照组点云数为: " << All_clouds_->size() << std::endl;
        pcl::visualization::CloudViewer V_duizhao("Viewer duizhao");
        V_duizhao.showCloud(All_clouds_);
        while (!V_duizhao.wasStopped())
        {
        }

        //下采样
        pcl::console::print_highlight("Downsampling...\n");
        pcl::VoxelGrid<PointNT> grid;
        const float leaf = 0.002f;
        grid.setLeafSize(leaf, leaf, leaf);
        grid.setInputCloud(object);
        grid.filter(*object);
        grid.setInputCloud(scene);
        grid.filter(*scene);

        // Estimate normals for scene
        pcl::console::print_highlight("Estimating scene normals...\n");
        pcl::NormalEstimationOMP<PointNT, PointNT> nest;
        nest.setRadiusSearch(0.01);
        nest.setInputCloud(scene);
        nest.compute(*scene);

        // Estimate features
        pcl::console::print_highlight("Estimating features...\n");
        FeatureEstimationT fest;
        fest.setRadiusSearch(0.02);
        fest.setInputCloud(object);
        fest.setInputNormals(object);
        fest.compute(*object_features);
        fest.setInputCloud(scene);
        fest.setInputNormals(scene);
        fest.compute(*scene_features);

        // Perform alignment
        pcl::console::print_highlight("Starting alignment...\n");
        pcl::SampleConsensusPrerejective<PointNT, PointNT, FeatureT> align;
        align.setInputSource(object);
        align.setSourceFeatures(object_features);
        align.setInputTarget(scene);
        align.setTargetFeatures(scene_features);
        align.setMaximumIterations(50000);               // Number of RANSAC iterations
        align.setNumberOfSamples(3);                     // Number of points to sample for generating/prerejecting a pose
        align.setCorrespondenceRandomness(5);            // Number of nearest features to use
        align.setSimilarityThreshold(0.9f);              // Polygonal edge length similarity threshold
        align.setMaxCorrespondenceDistance(2.5f * leaf); // Inlier threshold
        align.setInlierFraction(0.25f);                  // Required inlier fraction for accepting a pose hypothesis
        {
            pcl::ScopeTime t("Alignment");
            align.align(*object_aligned);
        }

        if (align.hasConverged())
        {
            // Print results
            printf("\n");
            Eigen::Matrix4f transformation = align.getFinalTransformation();
            pcl::console::print_info("    | %6.3f %6.3f %6.3f | \n", transformation(0, 0), transformation(0, 1), transformation(0, 2));
            pcl::console::print_info("R = | %6.3f %6.3f %6.3f | \n", transformation(1, 0), transformation(1, 1), transformation(1, 2));
            pcl::console::print_info("    | %6.3f %6.3f %6.3f | \n", transformation(2, 0), transformation(2, 1), transformation(2, 2));
            pcl::console::print_info("\n");
            pcl::console::print_info("t = < %0.3f, %0.3f, %0.3f >\n", transformation(0, 3), transformation(1, 3), transformation(2, 3));
            pcl::console::print_info("\n");
            pcl::console::print_info("Inliers: %i/%i\n", align.getInliers().size(), object->size());

            //将每一个点云融合到原始点云，用计算出来的RoT
            Fusion((Clouds[i]), All_clouds, transformation);
            std::cout << "实验组的点云数目是: " << All_clouds->size() << std::endl;
            pcl::visualization::CloudViewer V_all("Viewer All Clouds");
            V_all.showCloud(All_clouds);
            while (!V_all.wasStopped())
            {
            }

            // Show alignment
            pcl::visualization::PCLVisualizer visu("Alignment");
            visu.addPointCloud(scene, ColorHandlerT(scene, 0.0, 255.0, 0.0), "scene");
            visu.addPointCloud(object_aligned, ColorHandlerT(object_aligned, 0.0, 0.0, 255.0), "object_aligned");
            visu.spin();
        }
        else
        {
            pcl::console::print_error("Alignment failed!\n");
            return (1);
        }
    }
    return (0);
}